#include "GoodFeaturesToTrack.h"
#include "../utils/Logger.h"

GoodFeaturesToTrack::GoodFeaturesToTrack() {
    maxCornersCount = 500;
    mask = NULL;
}

GoodFeaturesToTrack::~GoodFeaturesToTrack() {
}

void GoodFeaturesToTrack::process(const IplImage* in) {
    bool releaseGray = false;
    IplImage *gray = (IplImage*) in;
    if (in->nChannels != 1) {
        gray = grayEffect.apply(in);
        releaseGray = true;
    }

    IplImage *eig = cvCreateImage(cvGetSize(in), IPL_DEPTH_32F, 1);
    IplImage *tmp = cvCreateImage(cvGetSize(in), IPL_DEPTH_32F, 1);

    cornersCount = maxCornersCount;
    corners = new CvPoint2D32f[ cornersCount ];

    cvGoodFeaturesToTrack(gray, eig, tmp, corners, &cornersCount, 0.01, 6.0, mask);

    cvFindCornerSubPix(gray, corners, cornersCount,
            cvSize(10, 10), cvSize(-1,-1),
            cvTermCriteria(CV_TERMCRIT_ITER|CV_TERMCRIT_EPS, 20, 0.03));

    cvReleaseImage(&eig);
    cvReleaseImage(&tmp);
    if (releaseGray)
        cvReleaseImage(&gray);    
}

IplImage* GoodFeaturesToTrack::apply(const IplImage* in) {
    Log << "Applying good features to track effect." << endl;

    process(in);

    IplImage *out = cvCreateImage(cvSize(in->width, in->height), in->depth, in->nChannels);
    cvCopy(in, out);

    for (int i = 0; i < cornersCount; ++i) {
        cvCircle(out, cvPoint(cvRound(corners[i].x), cvRound(corners[i].y)), 2, CV_RGB(255, 0, 0), CV_FILLED);
    }

    delete[] corners;

    return out;
}
